#include "../include/Track.hpp"
#define _DEBUG
namespace ORBSLAM
{

    Track::Track()
    {
    }
    Track::~Track()
    {
    }

    void Track::Set_param(Param *param)
    {
        mp_param = param;
        mp_multipleviewgeometry = new MultipleViewGeometry();
        mp_multipleviewgeometry->Set_param(mp_param);
        mi_itration_num = 200;
    }
    void Track::Track_frame()
    {
    }

    /**
     * @description:
     * @param v_kps1 keypoints
     * @param v_kps2
     * @param v_matches12
     * @return {*}
     */
    bool Track::Mono_initial_two_frame(vector<KeyPoint> &v_kps1, vector<KeyPoint> &v_kps2, vector<int> &v_matches12, Mat &R21, Mat &t21, vector<Point3f> &vp3f, vector<bool> &vb_triangulated, vector<int> &v_contribution_pt1, vector<int> &v_contribution_pt2, vector<pair<int, int>> &match_score)
    {
        vector<Match> matches12;
        matches12.reserve(v_kps2.size());
        vector<bool> matched1;
        matched1.resize(v_kps1.size());
        for (int i = 0; i < v_matches12.size(); i++)
        {
            if (v_matches12[i] >= 0)
            {
                matches12.push_back(make_pair(i, v_matches12[i]));
                matched1[i] = true;
            }
            else
            {
                matched1[i] = false;
            }
        }
        int n = matches12.size();
        vector<int> all_indices;
        all_indices.resize(n);
        vector<int> avaliable_indices;
        for (int i = 0; i < n; i++)
        {
            all_indices[i] = i;
        }
        vector<vector<int>> candidates = vector<vector<int>>(mi_itration_num, vector<int>(8, 0));
        srand((unsigned)time(NULL));
        for (int it = 0; it < mi_itration_num; it++)
        {
            avaliable_indices = all_indices;

            for (int j = 0; j < 8; j++)
            {
                int randi = rand() % (avaliable_indices.size());
                int idx = avaliable_indices[randi];
                if (match_score[matches12[idx].first].first <= 30)
                {
                    candidates[it][j] = idx;
                    avaliable_indices[randi] = avaliable_indices.back();
                    avaliable_indices.pop_back();
                }
                else
                {
                    j--;
                }
            }
        }

        vector<Point2f> vp2f_1, vp2f_2;
        vp2f_1.resize(matches12.size());
        vp2f_2.resize(matches12.size());
        for (int i = 0; i < matches12.size(); i++)
        {
            vp2f_1[i] = v_kps1[matches12[i].first].pt;
            vp2f_2[i] = v_kps2[matches12[i].second].pt;
        }

        if (0)
        {
            cout << "solve rt by opencv..." << endl;
            Mat mask;
            Mat cameraMatrix = Mat::eye(3, 3, CV_64F);
            Mat essential_matrix = findEssentialMat(vp2f_1, vp2f_2, mp_param->mm_camera_intrinsics, RANSAC, 0.999, 1.0, mask);

            vector<Point3f> v_p3d;

            Mat pts_4d;

            recoverPose(essential_matrix, vp2f_1, vp2f_2, mp_param->mm_camera_intrinsics, R21, t21, 1000, mask, pts_4d);
            cout << "R21:" << R21 << endl;
            cout << "t21" << t21 << endl;
            cout << "mask:" << mask << endl;
            cout << "pts_4d" << pts_4d << endl;

            vp3f.resize(v_kps1.size());
            int n = 0;
            vb_triangulated = vector<bool>(v_kps1.size(), false);
            for (int i = 0; i < pts_4d.cols; i++)
            {
                cout << "mask(i):" << i << ":" << mask.at<u_char>(i, 0) << endl;

                if (mask.at<u_char>(i, 0) == 1)
                {

                    Mat x = pts_4d.col(i);
                    cout << "x:" << x << endl;
                    double z = x.at<double>(3, 0);
                    cout << "z:" << z << endl;
                    Point3f P(x.at<double>(0, 0) / z, x.at<double>(1, 0) / z, x.at<double>(2, 0) / z);
                    vp3f[matches12[i].first] = P;
                    n++;
                    vb_triangulated[matches12[i].first] = true;
                }
            }
            cout << "triangulated points num:" << n << endl;
            return true;

            vector<bool> v_triangulated;
            float parallax;

            vector<bool> v_ifinline(vp2f_1.size(), true);

            R21.convertTo(R21, CV_32F);
            t21.convertTo(t21, CV_32F);

            int ngood = mp_multipleviewgeometry->CheckRT(R21, t21, vp2f_1, vp2f_2, v_ifinline, v_p3d, v_triangulated, parallax);
            cout << "ngood:" << ngood << endl;

            // vp3f.resize(v_kps1.size());
            // int n = 0;
            // vb_triangulated = vector<bool>(v_kps1.size(), false);
            for (int i = 0; i < matches12.size(); i++)
            {
                if (v_triangulated[i])
                {
                    vp3f[matches12[i].first] = v_p3d[i];
                    vb_triangulated[matches12[i].first] = true;
                    n++;
                }
            }
            cout << "triangulated points num:" << n << endl;
            return true;
        }

        int contribution_group;
        vector<bool> vb_inlineesH, vb_inlineesF;
        int ninlinersH, ninlinersF;
        Mat H21, F21;
        mp_multipleviewgeometry->Find_fundamental(vp2f_1, vp2f_2, matches12, mi_itration_num, candidates, F21, vb_inlineesF, ninlinersF, contribution_group);
        // mp_multipleviewgeometry->Find_fundamental_2(v_kps1,v_kps2,vp2f_1, vp2f_2, matches12, mi_itration_num, candidates, F21, vb_inlineesF, ninlinersF);

        v_contribution_pt1.resize(8);
        v_contribution_pt2.resize(8);
        for (int i = 0; i < 8; i++)
        {
            v_contribution_pt1[i] = matches12[candidates[contribution_group][i]].first;
            v_contribution_pt2[i] = matches12[candidates[contribution_group][i]].second;
        }

        cout << "ninlinersF:" << ninlinersF << endl;
        // cout << "F21:" << F21 << endl;
        vector<bool> vb_triangulated_inline;
        vector<Point3f> vp3f_inline;
        bool b_getRT = mp_multipleviewgeometry->ReconstructF(ninlinersF, vp2f_1, vp2f_2, vb_inlineesF, F21, R21, t21, vp3f_inline, vb_triangulated_inline, 1.0, 50);
        if (b_getRT)
        {
            cout << "R21:" << R21 << endl;
            cout << "t21" << t21 << endl;
            vp3f.resize(v_kps1.size());
            int n = 0;
            vb_triangulated = vector<bool>(v_kps1.size(), false);
            for (int i = 0; i < matches12.size(); i++)
            {
                if (vb_triangulated_inline[i])
                {
                    vp3f[matches12[i].first] = vp3f_inline[i];
                    vb_triangulated[matches12[i].first] = true;
                    n++;
                }
            }
            cout << "triangulated points num:" << n << endl;
        }
        return b_getRT;
    }

    // void Track::Set_ref_KF(KeyFrame *p_KF)
    // {
    //     mp_ref_KF = p_KF;
    // }

    bool Track::Track_reference_keyframe(KeyFrame *p_ref_KF, Frame &cur_frame, Matcher *p_matcher)
    {
        vector<MapPoint *> vp_mpt_match;
        int nmatches = p_matcher->Search_by_bow(p_ref_KF, cur_frame, vp_mpt_match);
        cout << "search bow, nmatches:" << nmatches << endl;
#ifdef _DEBUG
        // Debuger::Show_two_image_match(p_ref_KF, cur_frame, vp_mpt_match);
#endif // _DEBUG

        if (nmatches < 15)
        {
            cout << "track reference keframe, less than 15 matches" << endl;
            return false;
        }

        cur_frame.mvp_mappoints = vp_mpt_match;

        // Debuger::Show_two_image_match(p_ref_KF, cur_frame);

        // vector<Point3f> pts_3d;
        // vector<Point2f> pts_2d;
        // for (int i = 0; i < cur_frame.mvp_mappoints.size(); i++)
        // {
        //     if (cur_frame.mvp_mappoints[i])
        //     {
        //         Mat pose = cur_frame.mvp_mappoints[i]->Get_world_pose();
        //         pts_3d.push_back(Point3f(pose.at<float>(0), pose.at<float>(1), pose.at<float>(2)));
        //         pts_2d.push_back(cur_frame.mv_orb_unkeypoints[i].pt);
        //     }
        // }
        // Mat r, t, R, inliner;
        // solvePnPRansac(pts_3d, pts_2d, mp_param->mm_camera_intrinsics, Mat(), r, t, false, 300, 5.991, 0.99, inliner, SOLVEPNP_EPNP);
        // Rodrigues(r, R);
        // cout << "inliner:" << inliner << endl;
        // cout << "EPNP R,t:" << endl
        //      << R << endl
        //      << t << endl;

        // Mat Tcw = Mat::eye(4, 4, CV_32F);
        // R.copyTo(Tcw.rowRange(0, 3).colRange(0, 3));
        // t.copyTo(Tcw.rowRange(0, 3).col(3));
        // cur_frame.Set_pose(Tcw);

        // solvePnP(pts_3d, pts_2d, mp_param->mm_camera_intrinsics, Mat(), r, t, false, SOLVEPNP_ITERATIVE);
        // Rodrigues(r, R);
        // cout << "ITERATIVE R,t:" << endl
        //      << R << endl
        //      << t << endl;

        int n_inline = Optimizer::Pose_optimization(&cur_frame);
        // cout << "T by optimization:" << endl
        //      << cur_frame.mm_Tcw << endl;

        int n_succ_matches = 0;
        for (int i = 0; i < cur_frame.mn_feature_num; i++)
        {
            if (cur_frame.mvp_mappoints[i])
            {
                if (cur_frame.mvb_outline[i])
                {
                    // MapPoint *p_mpt = cur_frame.mvp_mappoints[i];

                    cur_frame.mvp_mappoints[i] = static_cast<MapPoint *>(NULL);
                    // cur_frame.mn_mpts_num--;
                    cur_frame.mvb_outline[i] = false;
                }
                else if (cur_frame.mvp_mappoints[i]->Get_observation_num() > 0)
                {
                    n_succ_matches++;
                }
            }
        }
        cout << "track reference keyframe, ninlines:" << n_succ_matches << endl;
        return true;
    }

    bool Track::Track_by_motion_predict(Frame &last_frame, Frame &cur_frame, Matcher *p_matcher, Mat &velocity)
    {
        const Mat &pose_pre = velocity * last_frame.Get_Tcw();
        cur_frame.Set_pose(pose_pre);
        // cout<<"velocity:"<<velocity<<endl;
        // cout<<"last_frame Tcw:"<<last_frame.Get_Tcw()<<endl;
        // cout<<"pose pre:"<<pose_pre<<endl;

        fill(cur_frame.mvp_mappoints.begin(), cur_frame.mvp_mappoints.end(), static_cast<MapPoint *>(NULL));

        int th = 15;

        int nmatches = p_matcher->Search_by_projection(last_frame, cur_frame, th);

        if (nmatches < 20)
        {
            fill(cur_frame.mvp_mappoints.begin(), cur_frame.mvp_mappoints.end(), static_cast<MapPoint *>(NULL));
            nmatches = p_matcher->Search_by_projection(last_frame, cur_frame, 2 * th);
        }
        if (nmatches < 20)
        {
            cur_frame.Set_pose(last_frame.Get_Tcw());
            fill(cur_frame.mvp_mappoints.begin(), cur_frame.mvp_mappoints.end(), static_cast<MapPoint *>(NULL));
            nmatches = p_matcher->Search_by_projection(last_frame, cur_frame, 2 * th);
        }
        if(nmatches<20){
            return false;
        }
        Optimizer::Pose_optimization(&cur_frame);
        // cout << "T by optimization:" << endl
        //      << cur_frame.Get_Tcw() << endl;

        int ninline = 0;
        for (int i = 0; i < cur_frame.mn_feature_num; i++)
        {
            if (cur_frame.mvp_mappoints[i])
            {
                if (cur_frame.mvb_outline[i])
                {
                    // MapPoint * p_mpt = cur_frame.mvp_mappoints[i];
                    cur_frame.mvp_mappoints[i] = static_cast<MapPoint *>(NULL);
                    cur_frame.mvb_outline[i] = false;
                }
                else if (cur_frame.mvp_mappoints[i]->Get_observation_num() > 0)
                {
                    ninline++;
                }
            }
        }
        cout << "motion track ninline:" << ninline << endl;
        return ninline > 15;
    }

    bool Track::Track_local_map(LocalMap *p_localmap, Frame &cur_frame, Matcher *p_matcher, int &nmatch_inline)
    {
        Search_local_match(p_localmap, cur_frame, p_matcher);

        // vector<Point3f> pts_3d;
        // vector<Point2f> pts_2d;
        // for (int i = 0; i < cur_frame.mvp_mappoints.size(); i++)
        // {
        //     if (cur_frame.mvp_mappoints[i])
        //     {
        //         Mat pose = cur_frame.mvp_mappoints[i]->Get_world_pose();
        //         pts_3d.push_back(Point3f(pose.at<float>(0), pose.at<float>(1), pose.at<float>(2)));
        //         pts_2d.push_back(cur_frame.mv_orb_unkeypoints[i].pt);
        //     }
        // }
        // Mat r, t, R;
        // solvePnP(pts_3d, pts_2d, mp_param->mm_camera_intrinsics, Mat(), r, t, false, SOLVEPNP_EPNP);
        // Rodrigues(r, R);
        // cout << "EPNP R,t:" << endl
        //      << R << endl
        //      << t << endl;

        // Mat Tcw = Mat::eye(4, 4, CV_32F);
        // R.copyTo(Tcw.rowRange(0, 3).colRange(0, 3));
        // t.copyTo(Tcw.rowRange(0, 3).col(3));
        // cur_frame.Set_pose(Tcw);

        // solvePnP(pts_3d, pts_2d, mp_param->mm_camera_intrinsics, Mat(), r, t, false, SOLVEPNP_ITERATIVE);
        // Rodrigues(r, R);
        // cout << "ITERATIVE R,t:" << endl
        //      << R << endl
        //      << t << endl;

        Optimizer::Pose_optimization(&cur_frame);
        // cout << "T by optimization:" << endl
        //      << cur_frame.mm_Tcw << endl;

        nmatch_inline = 0;

        for (int i = 0; i < cur_frame.mn_feature_num; i++)
        {
            if (cur_frame.mvp_mappoints[i])
            {
                if (!cur_frame.mvb_outline[i])
                {
                    cur_frame.mvp_mappoints[i]->Increase_found(1);
                    if (cur_frame.mvp_mappoints[i]->Get_observation_num() > 0)
                    {
                        nmatch_inline++;
                    }
                }
                else
                {
                    cur_frame.mvp_mappoints[i] = static_cast<MapPoint *>(NULL);
                    // cur_frame.mn_mpts_num--;
                    cur_frame.mvb_outline[i] = false;
                }
            }
        }
        cout << "after track localmap ninline..................................................................." << nmatch_inline << endl;
        if (nmatch_inline < 30)
        {
            cout << "after track localmap, tracked mappoints less then 30, track lost." << endl;
            return false;
        }
        else
        {
            return true;
        }
    }

    bool Track::Relocalization(Frame &cur_frame, KeyFrameDatabase *p_keyframe_database, Matcher *p_matcher, Map *p_map)
    {
        vector<KeyFrame *> vp_candidate_keyframe = p_keyframe_database->Detect_relocalization_candidates(&cur_frame, p_map);
        if (vp_candidate_keyframe.empty())
        {
            cout << "no candidate keyframe for relocalization." << endl;
            return false;
        }
        const int n_keyframe = vp_candidate_keyframe.size();
        p_matcher->Set_ratio(0.75);
        p_matcher->Set_if_check_orientation(true);

        vector<vector<MapPoint *>> vvp_mappoint_match;
        vvp_mappoint_match.resize(n_keyframe);

        vector<bool> vb_mark_discard;
        vb_mark_discard.resize(n_keyframe);
        int n_candidates = 0;

        for (int i = 0; i < n_keyframe; i++)
        {
            KeyFrame *p_keyframe = vp_candidate_keyframe[i];
            if (p_keyframe->Is_bad())
            {
                vb_mark_discard[i] = true;
            }
            else
            {
                int nmatch = p_matcher->Search_by_bow(p_keyframe, cur_frame, vvp_mappoint_match[i]);
                if (nmatch < 15)
                {
                    vb_mark_discard[i] = true;
                    continue;
                }
                else
                {
                    n_candidates++;
                    vb_mark_discard[i] = false;
                }
            }
        }

        bool b_match = false;
        p_matcher->Set_ratio(0.9);

        while (n_candidates > 0 && !b_match)
        {
            for (int i = 0; i < n_keyframe; i++)
            {
                if (vb_mark_discard[i])
                {
                    continue;
                }

                vector<Point3f> pts_3d;
                vector<Point2f> pts_2d;

                vector<int> v_tmp;
                v_tmp.reserve(vvp_mappoint_match[i].size());
                for (int j = 0; j < vvp_mappoint_match[i].size(); j++)
                {
                    if (vvp_mappoint_match[i][j])
                    {
                        Mat pose = vvp_mappoint_match[i][j]->Get_world_pose();
                        pts_3d.push_back(Point3f(pose.at<float>(0), pose.at<float>(1), pose.at<float>(2)));
                        pts_2d.push_back(cur_frame.mv_orb_unkeypoints[j].pt);
                        v_tmp.push_back(j);
                    }
                }
                Mat r, t, R, inliner;
                solvePnPRansac(pts_3d, pts_2d, mp_param->mm_camera_intrinsics, Mat(), r, t, false, 300, 5.991, 0.99, inliner, SOLVEPNP_EPNP);

                Rodrigues(r, R);
                cout << "inliner:" << inliner << endl;
                cout << "EPNP R,t:" << endl
                     << R << endl
                     << t << endl;

                Mat Tcw = Mat::eye(4, 4, CV_32F);
                R.copyTo(Tcw.rowRange(0, 3).colRange(0, 3));
                t.copyTo(Tcw.rowRange(0, 3).col(3));
                cur_frame.Set_pose(Tcw);

                set<MapPoint *> sp_mpt_found;

                for (int k = 0; k < inliner.rows; k++)
                {
                    int index = v_tmp[inliner.at<int>(k, 0)];
                    cur_frame.mvp_mappoints[index] = vvp_mappoint_match[i][index];
                    sp_mpt_found.insert(vvp_mappoint_match[i][index]);
                }

                int n_good = Optimizer::Pose_optimization(&cur_frame);

                if (n_good < 10)
                {
                    continue;
                }

                for (int io = 0; io < cur_frame.mn_feature_num; io++)
                {
                    if (cur_frame.mvb_outline[io])
                    {
                        cur_frame.mvp_mappoints[io] = static_cast<MapPoint *>(NULL);
                        cur_frame.mvb_outline[io] = false;
                    }
                }

                if (n_good < 50)
                {
                    int n_additional = p_matcher->Search_by_projection_relocalization(cur_frame, vp_candidate_keyframe[i], sp_mpt_found, 10, 100);
                    if (n_additional + n_good >= 50)
                    {
                        n_good = Optimizer::Pose_optimization(&cur_frame);
                        if (n_good > 30 && n_good < 50)
                        {
                            sp_mpt_found.clear();
                            for (int ip = 0; ip < cur_frame.mn_feature_num; ip++)
                            {
                                if (cur_frame.mvp_mappoints[ip])
                                {
                                    sp_mpt_found.insert(cur_frame.mvp_mappoints[ip]);
                                }
                            }
                            n_additional = p_matcher->Search_by_projection_relocalization(cur_frame,vp_candidate_keyframe[i],sp_mpt_found,3,64);

                            if(n_good+n_additional>=50){
                                n_good = Optimizer::Pose_optimization(&cur_frame);
                                for(int ip = 0;ip<cur_frame.mn_feature_num;ip++){
                                    if(cur_frame.mvb_outline[ip]){
                                        cur_frame.mvp_mappoints[ip] = static_cast<MapPoint*>(NULL);
                                    }
                                }
                            }
                        }
                    }
                }

                if(n_good>=50){
                    b_match = true;
                    break;
                }
            }
        }

        if(!b_match){
            return false;
        }
        else{
            return true;
        }
    }

    void Track::Search_local_match(LocalMap *p_local_map, Frame &cur_frame, Matcher *p_matcher)
    {
        for (vector<MapPoint *>::iterator vit = cur_frame.mvp_mappoints.begin(); vit != cur_frame.mvp_mappoints.end(); vit++)
        {
            MapPoint *p_mpt = *vit;
            if (p_mpt)
            {
                if (p_mpt->Is_bad())
                {
                    *vit = static_cast<MapPoint *>(NULL);
                }
                else
                {
                    p_mpt->Increase_visible(1);
                    p_mpt->mn_lastseen_frameid = cur_frame.mn_frame_id;
                }
            }
        }

        int ntomatch = 0;
        for (auto p_mpt : p_local_map->mvp_local_mappoint)
        {
            if (p_mpt->mn_lastseen_frameid == cur_frame.mn_frame_id)
            {
                p_mpt->mb_ifproj_tocurframe = false;
                continue;
            }
            if (cur_frame.Is_inframe_field(p_mpt, 0.5))
            {
                ntomatch++;
                p_mpt->Increase_visible(1);
                cur_frame.mmap_mappoints_to_project[p_mpt->mn_id] = p_mpt->m_preproj_uv_tocurframe;
            }
            else
            {
                p_mpt->mb_ifproj_tocurframe = false;
            }
        }
        cout << "ntomatch:" << ntomatch << endl;
        if (ntomatch > 0)
        {
            // TODO
            int th = 10;
            p_matcher->Set_ratio(0.8);
            int matches = p_matcher->Search_by_projection(cur_frame, p_local_map->mvp_local_mappoint, th, false);
            cout << "num new match:" << matches << endl;
        }
    }
}